//
// Created by Pulsar on 2020/5/16.
//

#include <rc_task/rcMoveTask.h>
#include <rc_move/rcmove.h>
#include <rc_log/slog.hpp>
#include <iostream>
#include <rc_gpio/c_gpio.h>
#include <rc_gpio/soft_pwm.h>
#include <rc_message/rc_msg_server.h>
#include <utility>
#include <vector>
#include <rc_task/rcTaskVariable.h>


namespace rccore {
    namespace task {
        MoveTask::MoveTask(std::shared_ptr<common::Context> pcontext) : RobotCarMove(std::move(pcontext)) {

        }

        void MoveTask::Run() {
            using namespace rccore::message;
            slog::info << "制动任务启动中" << slog::endl;
//            RobotCarMove robot(pcontext);
            this->init();
            m_thread = new std::thread([&]() {
                task::task_variable::TASK_NUM += 1;
                while (not m_isStop) {
                    std::unique_lock<std::mutex> m_locker(m_mutex);
                    m_cv.wait(m_locker, [this] { return !m_isPause; });
                    if (pcontext->pmessage_server->pmoveMessage->size_message() != 0) {
                        WHEEL_DATA wheelData = pcontext->pmessage_server->pmoveMessage->front_message();
                        this->excute(wheelData);
                        pcontext->pmessage_server->pmoveMessage->pop_message();
                    }
                    m_locker.unlock();//m_locker unlock
                }
                this->wheel_stop();
                slog::warn << "制动任务退出" << slog::endl;
                task::task_variable::TASK_NUM -= 1;
            });
        }

        MoveTask::~MoveTask() {

        }

        void MoveTask::Stop() {
            m_isStop = true;
        }
    }
}
